#include "motor.h"
#include "motor.h"
#include "motor.h"
#include <iostream>
#include <thread>

#include <boost/thread.hpp>
#include <boost/algorithm/hex.hpp>

using namespace std ;
using namespace boost::asio;

// 毫秒
typedef std::chrono::time_point<std::chrono::steady_clock, std::chrono::duration<double, std::ratio<1, 1000>>> Millsecond ;

unsigned char* convertIntToHex(int num){
    unsigned char* _cmd ;
    int count =2;
    for ( int i = 0; i < count; ++i ) {
        _cmd[count-i] = num % 256 ;
        num >>= 8 ;
    }
    return _cmd;
}

void handler(const boost::system::error_code &error, size_t bytes_transferred)
{
        if (!error) {
            //cout << bytes_transferred << endl;
        }else {
            cerr <<__FUNCTION__<< error.message() << endl;
        }
}


Motor::Motor()
{
    memset( _oneBuffer, 0, sizeof(_oneBuffer));
    preConnectDevice();
}

Motor::~Motor()
{
    disconnectDevice();
}

void Motor::sendCMDs(std::vector<uchar> &cmds)
{
    std::lock_guard<std::recursive_mutex> lck(_mtx);
    if(!_serialPort_ptr)
        return;
    if(cmds.empty()){
        throw EXP_MOTOR_VALUE;
    }
    try{
        boost::asio::write(*_serialPort_ptr, buffer(cmds, cmds.size()));
        _bReply.store(false);
        waitReply() ;
    }
    catch(const boost::exception &e){
        _condReply.notify_all();
    }
}


void Motor::sendCMDs(std::vector<u16> &cmds)
{
    std::lock_guard<std::recursive_mutex> lck(_mtx);
    if(!_serialPort_ptr)
        return;
    if(cmds.empty()){
        throw EXP_MOTOR_VALUE;
    }
    try{
        boost::asio::write(*_serialPort_ptr, buffer(cmds, cmds.size()));
        _bReply.store(false);
        waitReply() ;
    }
    catch(const boost::exception &e){
        _condReply.notify_all();
    }
}

void Motor::sendCMDs(std::vector<ulong> &cmds)
{
    std::lock_guard<std::recursive_mutex> lck(_mtx);
    if(!_serialPort_ptr)
        return;
    if(cmds.empty()){
        throw EXP_MOTOR_VALUE;
    }
    try{
        boost::asio::write(*_serialPort_ptr, buffer(cmds, cmds.size()));
        _bReply.store(false);
        waitReply() ;
    }
    catch(const boost::exception &e){
        _condReply.notify_all();
    }
}

void Motor::sendCMDs(u16 *cmds)
{
    std::lock_guard<std::recursive_mutex> lck(_mtx);
    if(!_serialPort_ptr)
        return;
//    if(cmds.empty()){
//        throw EXP_MOTOR_VALUE;
//    }
    try{
        printf("%4X \n",*cmds);
        std::cout << sizeof(*cmds) << "\n" <<std::endl;
        boost::asio::write(*_serialPort_ptr, buffer(cmds, sizeof(cmds)));
        _bReply.store(false);
        waitReply() ;
    }
    catch(const boost::exception &e){
        _condReply.notify_all();
    }
}

// 重连设备
void Motor::reconnectDevice()
{
    getDesc();
    disconnectDevice();
    connectDevice(port_name,port_bau);
}



// 做链接的准备工做
void Motor::preConnectDevice()
{
    try {
        if ( !_serialPort_ptr ) {
            _serialPort_ptr.reset( new  serial_port(_service));
        }else{
            qWarning("注意，串口资源上次未释放!");
        }
    }
    catch ( const boost::system::system_error &e ) {
        std::cout <<"Motor Pre Connect Failed!" <<"system:"<< e.what() << std::endl ;
    }
    catch ( const boost::exception &e ) {
        std::cout <<"Motor Pre Connect Failed! exception:" << std::endl ;
    }
    getDesc();
}

//连接设备
void Motor::connectDevice(string name , uint port)
{
    std::lock_guard<std::recursive_mutex> lck(_mtx) ;
    if(isDeviceOpend()) return;
    try {
            _serialPort_ptr->open(name);
            _serialPort_ptr->set_option(serial_port::baud_rate(port));
            _serialPort_ptr->set_option(serial_port::flow_control(serial_port::flow_control::none)) ;
            _serialPort_ptr->set_option(serial_port::parity(serial_port::parity::none)) ;
            _serialPort_ptr->set_option(serial_port::stop_bits(serial_port::stop_bits::one)) ;
            _serialPort_ptr->set_option(serial_port::character_size(serial_port::character_size(8))) ;

            boost::asio::async_read( *_serialPort_ptr, boost::asio::buffer(_oneBuffer),
                                     boost::bind( &Motor::recvReply, this, _1, _2 ) ) ;

            _thService = std::thread([this]() {_service.run();}) ;

#if PRINTCORRECT
            std::cout << "connect motor success\n" << std::flush ;
#endif

    }
    catch ( const boost::system::system_error &e ) {
        std::cout<<name<<":" <<"motor system:"<< e.what() << std::endl ;
        throw EXP_MOTOR_SYSTEM;
    }
    catch ( const boost::exception &e ) {
        std::cout <<"motor exception:" << std::endl ;
        throw EXP_MOTOR_SYSTEM;
    }
}

//断开设备
void Motor::disconnectDevice()
{
    std::lock_guard<std::recursive_mutex> lck(_mtx) ;
    if ( _serialPort_ptr ) {
        if ( _serialPort_ptr->is_open() ) {
            _service.stop() ;
            _serialPort_ptr->close();
            _thService.join() ;
        }
        _serialPort_ptr = nullptr;
    }
}

/**
 * @brief Motor::read_from_serial
 * @return
 * @warning 暂时放弃该主动从串口读取的方式
 */
QByteArray Motor::read_from_serial()
{
    unsigned char dd[11];
    QByteArray array;

//    _bReply.store(true);
//    _condReply.notify_one() ;


//    for ( size_t i = 0; i < sizeof(dd); ++i ) {
//        array.push_back(dd[i]);
//    }

//    std::this_thread::sleep_for( std::chrono::milliseconds(100) ) ;

//    boost::asio::async_read(*_serialPort_ptr,  boost::asio::buffer(dd),boost::bind( &Motor::recvReply, this, _1, _2 ));

    return array;
}






bool Motor::isDeviceOpend()
{
    std::lock_guard<std::recursive_mutex> lck(_mtx) ;
    return _serialPort_ptr->is_open();
}


void Motor::setBuffSize(int count)
{
    myBuffer.resize(count);
    myBuffer.clear();
}


void Motor::recvReply( const boost::system::error_code &ec, size_t )
{
    if ( !ec ) {
        std::string out ;
        boost::algorithm::hex(_oneBuffer, std::back_inserter(out));

        // std::cout << _oneBuffer << std::endl;
        myBuffer.push_back(_oneBuffer[0]);
        if(myBuffer.full()){
            recivers.clear();
            while(!myBuffer.empty()){
                // 获取到了一帧数据
                recivers.push_back(myBuffer.front());
                myBuffer.pop_front();
            }
            bufFull(recivers);
        }

        _bReply.store(true);
        _condReply.notify_one() ;

        boost::asio::async_read( *_serialPort_ptr, boost::asio::buffer(_oneBuffer),
                                 boost::bind( &Motor::recvReply, this, _1, _2 ) ) ;


    }else {
        qWarning("Receive ec warning!");
    }
}

void Motor::waitReply()
{
    using namespace std::chrono ;
    Millsecond m1 = steady_clock::now() ;
    std::unique_lock<std::mutex> lck(_mtxReply) ;
    // 考虑到异常情况，设置超时时间
    _condReply.wait_for( lck, std::chrono::milliseconds(100), [this]{return _bReply.load() ;}) ;
    Millsecond m2 = steady_clock::now() ;
    // std::cout << "Com通讯时间--->" << (m2 - m1).count() << std::endl ;
    std::this_thread::sleep_for( std::chrono::milliseconds(50) ) ;
}

void Motor::getDesc()
{
    Fast::config app("conf/serial.json");
    port_name =  app.get<std::string>("port");
    port_bau =  app.get<uint>("bau");
    //if(getConnecting()) disconnectDevice();
#ifdef Q_OS_LINUX
    port_name = "/dev/"+port_name;
#elif
#endif
}




